/**
 * @file   globalfunctions.h
 * @brief  global functions
 * @author Changyuan Zhao
 * @date  Dec 2021
 * @version 1.0
 * 
 * Reference:
 *   CORA ../global/functions
 */

#ifndef GLOBALFUNCTIONS_H
#define GLOBALFUNCTIONS_H

#include <iostream>
#include "BasicObject.h"
#include <numeric>
namespace reachSolver{

/**
 * @brief: sort rows of a matrix in ascending order as a group
 * @param A a matrix must be a 2-D matrix
 * @return I the index
 */
template <typename Number>
std::vector<size_t> sortrows(Matrix_t<Number> A){
    std::vector<size_t> idx(A.rows());
    std::iota(idx.begin(), idx.end(), 0);
    std::sort(idx.begin(),idx.end(),
              [&A](size_t i1, size_t i2) {return A(i1,0)<A(i2,0);});
    return idx;
}

/**
 * @brief: add up all generators that belong to terms with identical exponents
 * @param ExpMat matrix containing the exponent vectors
 * @param G generator matrix
 * @param ExpMatNew modified exponent matrix
 * @param Gnew modified generator matrix
 */
template <typename Number>
void removeRedundantExponents(Matrix_t<int> ExpMat, Matrix_t<Number> G, Matrix_t<int>& ExpMatNew,Matrix_t<Number>& Gnew){
    //remove zero-length generators
    Eigen::MatrixXd idxD = G.colwise().any();


    //skip if all non-zero
    if (~(idxD.all())){
        //if((~idxD).all()){
        if(idxD.prod() == 1){
            ExpMatNew.setZero(ExpMat.rows(), 1);
            Gnew = Eigen::MatrixXd::Zero(G.rows(), 1);
            return ;
        }
        else{
            auto j=0;
            Matrix_t<Number> tempG(G.rows(),G.cols());
            for(auto i=0;i<G.cols();i++){
                if(idxD(i) == true){
                    tempG.col(j) = G.col(i);
                    j++;
                }
            }
            tempG.conservativeResize(G.rows(),j);
            j=0;
            Matrix_t<int> tempExpMat(ExpMat.rows(),ExpMat.cols());
            for(auto i=0;i<ExpMat.cols();i++){
                if(idxD(i) == true){
                    tempExpMat.col(j) = ExpMat.col(i);
                    j++;
                }
            }
            tempExpMat.conservativeResize(ExpMat.rows(),j);
        }
    }

    //add hash value of the exponent vectors to the exponent matrix
    Matrix_t<int> temp(1,ExpMat.rows());
    for(auto i=0;i<ExpMat.rows();i++){
        temp(0,i) = (int)i+1;
    }
    Matrix_t<int> temp2 = (temp * ExpMat).adjoint();
    Matrix_t<int> rankMat(temp2.rows(),temp2.cols()+ExpMat.rows());
    rankMat << temp2, ExpMat.adjoint();
    
    //sort the exponent vectors according to the hash value
    std::vector<size_t> index = sortrows(rankMat);
    Matrix_t<int> ExpMatTemp(ExpMat.rows(),ExpMat.cols());
    for(auto i=0;i<ExpMat.cols();i++){
        ExpMatTemp.col(i) = ExpMat.col(index[i]);
    }
    //Matrix_t<Number> ExpMatTemp = ExpMat(index);
    Matrix_t<Number> Gtemp(G.rows(),G.cols());
    for(auto i=0;i<G.cols();i++){
        G.col(i) = G.col(index[i]);
    }

    //initialization 
    int counterNew = 0;
    ExpMatNew.setZero(ExpMat.rows(),ExpMat.cols());
    Gnew = Eigen::MatrixXd::Zero(G.rows(),G.cols());

    //first entry
    ExpMatNew.col(counterNew) = ExpMatTemp.col(0);
    Gnew.col(counterNew) = Gtemp.col(0);

    //loop over all exponent vectors
    for(auto counter = 1;counter<ExpMatTemp.cols();counter++){
        //flag = 0;
        //for(auto i=0;i<ExpMatNew.rows();i++){
        //    if(ExpMatNew.col(counterNew) == ExpMatTemp(counter))
        //}
        if((ExpMatNew.col(counterNew) == ExpMatTemp.col(counter))){
            Gnew.col(counterNew) = Gnew.col(counterNew) + Gtemp.col(counter);
        }
        else{
            counterNew++;
            Gnew.col(counterNew) = Gtemp.col(counter);
            ExpMatNew.col(counterNew) = ExpMatTemp.col(counter);
        }
    }

    //truncate exponent and generator matrix
    ExpMatNew = ExpMatNew.middleCols(0,counterNew+1);
    Gnew = Gnew.middleCols(0,counterNew+1);

}

/**
 * @brief: filters out generators of length0
 * @param G a matrix
 * @return reduced matrix of generators
 */
template <typename Number>
Matrix_t<Number> nonzeroFilter(Matrix_t<Number> G){
    Matrix_t<Number> result(G.rows(),G.cols());
    Vector_t<bool> index;
    index.setZero(G.cols(),1);
    for(auto i=0;i<G.cols();i++){
        if(G.col(i).any()){
            index(i) = true;
        }
    }
    auto k = 0;
    for(auto i=0;i<G.cols();i++){
        if(index(i) == true){
            result.col(k) = G.col(i);
            k++;
        }
    }
    result.conservativeResize(result.rows(),k);
    return result;
}

/**
 * @brief: calculate the inf of interval matrix M
 * @param M intervalMatrix
 * @return a Matrix
 */
template <typename Number>
Matrix_t<Number> inf(IntervalMatrix<Number> M){
    Matrix_t<Number> result(M.rows(),M.cols());
    for(auto i=0;i< M.rows();i++){
        for(auto j=0;j<M.cols();j++){
            result(i,j) = M(i,j).leftBound();
        }
    }
    return result;
}

/**
 * @brief: calculate the sup of interval matrix M
 * @param M intervalMatrix
 * @return a Matrix
 */
template <typename Number>
Matrix_t<Number> sup(IntervalMatrix<Number> M){
    Matrix_t<Number> result(M.rows(),M.cols());
    for(auto i=0;i< M.rows();i++){
        for(auto j=0;j<M.cols();j++){
            result(i,j) = M(i,j).rightBound();
        }
    }
    return result;
}

/**
 * @brief: set interval matrix with inf and sup
 * @param  inf a matrix
 * @param  sup a matrix
 * @return intervalMatrix
 */
template <typename Number>
IntervalMatrix<Number> setIntervalMatrix(Matrix_t<Number> inf, Matrix_t<Number> sup){
    IntervalMatrix<Number> result(inf.rows(),inf.cols());
    for(auto i=0;i< inf.rows();i++){
        for(auto j=0;j<inf.cols();j++){
            result(i,j).setLeftBound(inf(i,j));
            result(i,j).setRightBound(sup(i,j));
        }
    }
    return result;
}

/**
 * @brief: set interval matrix with a matrix
 * @param  M a matrix
 * @return intervalMatrix
 */
template <typename Number>
IntervalMatrix<Number> setIntervalMatrix(Matrix_t<Number> M){
    IntervalMatrix<Number> result(M.rows(),M.cols());
    for(auto i=0;i< M.rows();i++){
        for(auto j=0;j<M.cols();j++){
            result(i,j).setLeftBound(M(i,j));
            result(i,j).setRightBound(M(i,j));
        }
    }
    return result;
}

/**
 * @brief: set interval matrix with inf and sup
 * @param  inf a Vector
 * @param  sup a Vector
 * @return intervalMatrix
 */
template <typename Number>
IntervalMatrix<Number> setIntervalMatrix(Vector_t<Number> inf, Vector_t<Number> sup){
    IntervalMatrix<Number> result(inf.rows(),inf.cols());
    for(auto i=0;i< inf.rows();i++){
        for(auto j=0;j<inf.cols();j++){
            result(i,j).setLeftBound(inf(i,j));
            result(i,j).setRightBound(sup(i,j));
        }
    }
    return result;
}

/**
 * @brief: set interval matrix with a Vector
 * @param  M a Vector
 * @return intervalMatrix
 */
template <typename Number>
IntervalMatrix<Number> setIntervalMatrix(Vector_t<Number> M){
    IntervalMatrix<Number> result(M.rows(),M.cols());
    for(auto i=0;i< M.rows();i++){
        for(auto j=0;j<M.cols();j++){
            result(i,j).setLeftBound(M(i,j));
            result(i,j).setRightBound(M(i,j));
        }
    }
    return result;
}

template <typename Number>
void setinf(IntervalMatrix<Number>& M, Matrix_t<Number> inf){
    if(M.size() == 0){
        M.resize(inf.rows(),inf.cols());
    }
    for(auto i=0;i< M.rows();i++){
        for(auto j=0;j<M.cols();j++){
            M(i,j).setLeftBound(inf(i,j));
        }
    }
}

template <typename Number>
void setsup(IntervalMatrix<Number>& M, Matrix_t<Number> sup){
    if(M.size() == 0){
        M.resize(sup.rows(),sup.cols());
    }
    for(auto i=0;i< M.rows();i++){
        for(auto j=0;j<M.cols();j++){
            M(i,j).setLeftBound(sup(i,j));
        }
    }
}

/**
 * @brief: Merge the ID vectors of two polyZonotope objects and adpapte the exponent matrices accordingly
 * @param id1 IDvector of first polynomial zonotope
 * @param id2 IDvector of second polynomial zonotope
 * @param expMat1 exponent matrix of the first polynomial zonotope
 * @param expMat2 exponent matrix of the second polynomial zonotope
 * @param Mat1 adapted xponent matrix of the first polynomial zonotope
 * @param Mat2 adapted exponent matrix of the second polynomial zonotope
 * @return id merged of first polynomial zonotope
 */
template <typename Number>
std::vector<size_t> mergeExpMatrix(std::vector<size_t> id1, std::vector<size_t> id2, Matrix_t<int> expMat1, Matrix_t<int> expMat2, Matrix_t<int> &Mat1, Matrix_t<int> &Mat2){
    
    int L1 = id1.size();
    int L2 = id2.size();

    std::vector<size_t> id;

    //ID vector are identical 
    if(L1 == L2 && id1 == id2){
        id = id1;
        Mat1 = expMat1;
        Mat2 = expMat2;
    }
    //ID vectors not identical->merge
    else{

        //merge the two sets
        id = id1;
        Vector_t<size_t> ind2;
        ind2.setZero(id2.size());
        for(auto i=0;i<id2.size();i++){
            int ind = -1;
            for(auto j=0;j<id.size();j++){
                if(id[j] == id2[i]){
                    ind = j;
                    break;
                }
            }
            if(ind == -1){
                id.push_back(id2[i]);
                ind2(i) = id.size()-1;
            }
            else{
                ind2(i) = ind;
            }
        }

        //construct the new exponent matrices

        int L = id.size();
        Mat1.resize(expMat1.rows()+L-L1, expMat1.cols());
        Matrix_t<int> tempMa1;

        if((L-L1) != 0){
            tempMa1.setZero(L-L1, expMat1.cols());
            Mat1<< expMat1,
                   tempMa1;
        }
        else{
            Mat1 << expMat1;
        }
        Matrix_t<int> temp;
        temp.setZero(L, expMat2.cols());

        for(auto i=0;i<expMat2.rows();i++){
            temp.row(ind2(i)) = expMat2.row(i);
        }
        Mat2 = temp;
    }
    return id;
}


template <typename Number>
std::ostream & operator << (std::ostream & os, const Vector_t<Number> & vec){
    if(vec.rows()!=0){
        os<<"vector: deminsion "<<vec.rows()<<std::endl;
        for(auto i=0; i<vec.rows();i++){
            os<<vec(i,0)<<" ";
        }
        os<<std::endl;
    }
    return os;
}

template <typename Number>
std::ostream & operator << (std::ostream & os, const Matrix_t<Number> & mat){
    if(mat.rows()!=0){
        os<<"matrix: rows and cols "<<mat.rows()<<" "<<mat.cols()<<std::endl;
        for(auto i=0; i<mat.rows();i++){
            for(auto j=0; j<mat.cols();j++){
                os<<mat(i,j)<<" ";
            }
            os<<std::endl;
        }
    }
    return os;
}

template <typename Number>
std::ostream & operator << (std::ostream & os, const IntervalMatrix<Number> & mat){
    if(inf(mat).rows()!=0){
        os<<"interval matrix: inf"<<std::endl;
        os<<inf(mat)<<std::endl;
        os<<"interval matrix: sup"<<std::endl;
        os<<sup(mat)<<std::endl;
    }
    return os;
}


}// namespace reachSolver
#endif //POLYZONOTOPE_H